#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    FlipperGyro,    sensorGyro)
#pragma config(Sensor, in2,    ArmPot,         sensorPotentiometer)
#pragma config(Sensor, in3,    FlipperPot,     sensorPotentiometer)
#pragma config(Sensor, in4,    Auton_Pot,      sensorPotentiometer)
#pragma config(Sensor, in5,    Gyro,           sensorNone)
#pragma config(Sensor, dgtl1,  JawChanger,     sensorTouch)
#pragma config(Sensor, dgtl11, LED,            sensorLEDtoVCC)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_4,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_5,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_6,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           RightBaseBack, tmotorVex393HighSpeed, PIDControl, reversed, encoder, encoderPort, I2C_2, 1000)
#pragma config(Motor,  port2,           LeftArm,       tmotorVex393HighSpeed, PIDControl, reversed, encoder, encoderPort, I2C_4, 1000)
#pragma config(Motor,  port3,           RightArm,      tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port4,           LeftBaseFront, tmotorVex393, openLoop)
#pragma config(Motor,  port5,           TheJaw,        tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port6,           FlipperMotorLeft, tmotorVex269, openLoop, reversed, encoder, encoderPort, I2C_5, 1000)
#pragma config(Motor,  port7,           FlipperMotorRight, tmotorVex269, openLoop, encoder, encoderPort, I2C_6, 1000)
#pragma config(Motor,  port8,           LeftBaseBack,  tmotorVex393, openLoop)
#pragma config(Motor,  port9,           Convayer,      tmotorVex393, openLoop)
#pragma config(Motor,  port10,          RightBaseFront, tmotorVex393HighSpeed, PIDControl, reversed, encoder, encoderPort, I2C_1, 1000)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//


//#pragma config(Motor,  port5,           TheJaw,        tmotorVex269, openLoop, reversed)

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!


#define SpeedSlow 60
#define SpeedModerate 75
#define SpeedFast 80

////////////////////////////////////////////////////
//			FLIPPER POTENTIOMETER SETTINGS
////////////////////////////////////////////////////
#define FlipperToFloor      3300//3000//2700//3260/2850//2600//2500//2560
#define FlipperScore       	830
#define FlipperUp45Degrees  3640
#define FlipperScoreHigh   	2000


////////////////////////////////////////////////////
//			POTENTIOMETER ARM SETTINGS
////////////////////////////////////////////////////
#define ArmsFloor 1060
#define ArmsTrough 2222//2260
#define ArmsHigh 2780
// ARMS HOLD VALUE
#define ArmsMotorHold 10


////////////////////////////////////////////////////
//			Programming Skills Functions
////////////////////////////////////////////////////

void flippergodown()
{
	motor(FlipperMotorLeft) = -63;
	motor(FlipperMotorRight) = -63;
	wait1Msec(500);
	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;
	while (SensorValue [FlipperPot] > FlipperToFloor)
	{
		motor(FlipperMotorLeft) = -63;
		motor(FlipperMotorRight) = -63;
	}

	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;
}

void flippertotrough()
{
	// first reead the pot on flipper into a local variable
	// keep movimg the flliper down until it reaCHES  the "trough" pot setting
	// once it reaches the trough setting - quit moving the flipper (open jaws?)
	while (SensorValue[FlipperPot] > FlipperScore)
	{
		motor(FlipperMotorRight) = -35;
		motor(FlipperMotorLeft) = -35;
	}

	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;
}

task flipperup()
{
	while (SensorValue [FlipperPot] < 2600 /*3000*/)
	{
		motor(FlipperMotorLeft) = 127;
		motor(FlipperMotorRight) = 127;
		wait1Msec(10);
		motor(FlipperMotorLeft) = 10;
		motor(FlipperMotorRight) = 10;
		wait1Msec(10);
	}
	motor(FlipperMotorLeft) = 10;
	motor(FlipperMotorRight) = 10;
}

void flipperhigh(bool bCloseJaws)
{
	if (bCloseJaws)
	{
		//motor(TheJaw) = -127;
	}
	while (SensorValue [FlipperPot] < FlipperScoreHigh)
	{
		motor(FlipperMotorLeft) = 83;
		motor(FlipperMotorRight) = 83;
	}
	motor(FlipperMotorLeft) = 10;
	motor(FlipperMotorRight) = 10;
	//	motor(TheJaw) = 0;
}

void flipperscorehigh()
{
	while (SensorValue[FlipperPot] > FlipperScoreHigh)
	{
		motor(FlipperMotorRight) = -60; //83
		motor(FlipperMotorLeft) = -60; //83
	}

	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;
	wait1Msec(500);
}

task TASKflippertofloorandhold
{
	// raising up from trough position scored
	while (SensorValue [FlipperPot] < FlipperToFloor)
	{
		motor(FlipperMotorLeft) = 127;
		motor(FlipperMotorRight) = 127;
	}
	motor(FlipperMotorLeft) = 0;//10;
	motor(FlipperMotorRight) = 0;//10;
}

void MoveFlipperWithButtons()
{

	while(true)
	{
		// we will be in this loop until the user pushes a button
		// check if button has been pressed if so break out of loop
		if (nLCDButtons == 2)
		{
			// somebody pressed a button - break out of this loop and continue on...
			break;
		}
		if (nLCDButtons == 1)
		{
			motor[FlipperMotorLeft] = 80;
			motor[FlipperMotorRight] = 80;
			wait1Msec(100);
		}
		if (nLCDButtons == 4)
		{
			motor[FlipperMotorLeft] = -80;
			motor[FlipperMotorRight] = -80;
			wait1Msec(100);
		}
		motor[FlipperMotorLeft] = 10;
		motor[FlipperMotorRight] = 10;
	}

}


void ClearEncoders()
{
	nMotorEncoder[RightBaseBack] = 0;
	nMotorEncoder[RightBaseFront] = 0;
	nMotorEncoder[LeftBaseBack] = 0;
	nMotorEncoder[LeftBaseFront] = 0;
}


void WaitForLCD()
{

	while(true)
	{
		// we will be in this loop until the user pushes a button
		// check if button has been pressed if so break out of loop
		if (nLCDButtons != 0)
		{
			// somebody pressed a button - break out of this loop and continue on...
			break;
		}
	}
}

void RampUp(int iSpeed, int iDistance)
{
	// Ramp up as we start (the first 1/2 second - we should always move at least 1/2 second)
	int pwr = 1;
	for(int t = 1; t < iSpeed; t += 1)
	{
		// increment the power by t each time through the loop - starts at 50 and builds
		pwr = pwr + t;
		//set motors to pwr
		int iLE = abs(nMotorEncoder(LeftBaseBack));
		motor(LeftBaseBack) = pwr;
		motor(LeftBaseFront) = pwr;
		motor(RightBaseBack) = pwr;
		motor(RightBaseFront) = pwr;
		if (iLE > iDistance)
		{
			motor(LeftBaseBack) = 0;
			motor(LeftBaseFront) = 0;
			motor(RightBaseBack) = 0;
			motor(RightBaseFront) = 0;
			return;
		}
		if (pwr < 50)
		{
			wait1Msec(100);
		}
	}
}

void MoveForwardStraight(int iSpeed, int iLE, int iRE)
{
	if (iLE > iRE)
	{
		motor(LeftBaseBack) = iSpeed - 50;
		motor(LeftBaseFront) = iSpeed - 50;
		motor(RightBaseBack) = iSpeed;
		motor(RightBaseFront) = iSpeed;
	}
	else if (iRE > iLE)
	{
		motor(LeftBaseBack) = iSpeed;
		motor(LeftBaseFront) = iSpeed;
		motor(RightBaseBack) = iSpeed - 50;
		motor(RightBaseFront) = iSpeed - 50;
	}
	else
	{
		motor(LeftBaseBack) = iSpeed;
		motor(LeftBaseFront) = iSpeed;
		motor(RightBaseBack) = iSpeed;
		motor(RightBaseFront) = iSpeed;
	}
}

void MoveBackwardStraight (int iSpeed, int iLE, int iRE)
{
	if (iLE > iRE)
	{
		motor(LeftBaseBack) = iSpeed + 50;
		motor(LeftBaseFront) = iSpeed + 50;
		motor(RightBaseBack) = iSpeed;
		motor(RightBaseFront) = iSpeed;
	}
	else if (iRE > iLE)
	{
		motor(LeftBaseBack) = iSpeed;
		motor(LeftBaseFront) = iSpeed;
		motor(RightBaseBack) = iSpeed + 50;
		motor(RightBaseFront) = iSpeed + 50;
	}
	else
	{
		motor(LeftBaseBack) = iSpeed;
		motor(LeftBaseFront) = iSpeed;
		motor(RightBaseBack) = iSpeed;
		motor(RightBaseFront) = iSpeed;
	}
}

// bForward = true is forwards, false is backwards
//iDistance tells the encoder how far to go
void Move(bool bForward, int iDistance, int iSpeed)
{
	//Reset encoders
	nMotorEncoder(LeftBaseFront) = 0;
	nMotorEncoder(LeftBaseBack) = 0;
	nMotorEncoder(RightBaseFront) = 0;
	nMotorEncoder(RightBaseBack) = 0;
	// get the current value (will be zero to start cause we just reset it)
	int iLE = abs(nMotorEncoder(LeftBaseBack));
	int iRE = abs(nMotorEncoder(RightBaseBack));
	if (bForward)
	{
		// first ramp up slowly - then...
		RampUp(iSpeed, iDistance);
		// move forward until we reach encoder value given as iDistance
		while (iLE < iDistance)
		{
			MoveForwardStraight(iSpeed, iLE, iRE);
			iLE = abs(nMotorEncoder(LeftBaseBack));
			iRE = abs(nMotorEncoder(RightBaseBack));
		}
	}
	else
	{
		// we are moving backwards
		while (iLE < iDistance)
		{
			MoveBackwardStraight(-iSpeed, iLE, iRE);
			iLE = abs(nMotorEncoder(LeftBaseBack));
			iRE = abs(nMotorEncoder(RightBaseBack));
		}
	}
	motor(LeftBaseBack) = 0;
	motor(LeftBaseFront) = 0;
	motor(RightBaseBack) = 0;
	motor(RightBaseFront) = 0;
}

void MoveLowSpeed(bool bForwards, int iPosition)
{
	ClearEncoders();
	while (abs(nMotorEncoder(RightBaseBack)) < iPosition)
	{
		if (bForwards)
		{
			motor[RightBaseFront] = 50;
			motor[RightBaseBack] = 50;
			motor[LeftBaseFront] = 50;
			motor[LeftBaseBack] = 50;
		}
		else
		{
			motor[RightBaseFront] = -50;
			motor[RightBaseBack] = -50;
			motor[LeftBaseFront] = -50;
			motor[LeftBaseBack] = -50;
		}

	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;
}

void MoveMediumSpeed(bool bForwards, int iPosition)
{
	ClearEncoders();
	while (abs(nMotorEncoder(RightBaseBack)) < iPosition)
	{
		if (bForwards)
		{
			motor[RightBaseFront] = 80;
			motor[RightBaseBack] = 80;
			motor[LeftBaseFront] = 80;
			motor[LeftBaseBack] = 80;
		}
		else
		{
			motor[RightBaseFront] = -80;
			motor[RightBaseBack] = -80;
			motor[LeftBaseFront] = -80;
			motor[LeftBaseBack] = -80;
		}

	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;
}

void MoveFullSpeed(bool bForwards, int iPosition)
{
	ClearEncoders();
	while (abs(nMotorEncoder(RightBaseBack)) < iPosition)
	{
		if (bForwards)
		{
			motor[RightBaseFront] = 127;
			motor[RightBaseBack] = 127;
			motor[LeftBaseFront] = 127;
			motor[LeftBaseBack] = 127;
		}
		else
		{
			motor[RightBaseFront] = -127;
			motor[RightBaseBack] = -127;
			motor[LeftBaseFront] = -127;
			motor[LeftBaseBack] = -127;
		}

	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;
}

void Arms(bool bUp, int iPosition, int iMotorPower)
{
	//Move arms to trough, floor, or high
	// are we moving up or down
	if (bUp)
	{
		// we are moving up
		while (SensorValue[ArmPot] < iPosition)
		{
			motor(RightArm) = iMotorPower;
			motor(LeftArm) = iMotorPower;
		}
		// no movement - HOLD
		motor(RightArm) = ArmsMotorHold;
		motor(LeftArm) = ArmsMotorHold;
	}
	else if (!bUp)
	{
		// we are moving down
		while (SensorValue[ArmPot] > iPosition)
		{
			motor(RightArm) = -iMotorPower;
			motor(LeftArm) = -iMotorPower;
		}
		// no movement - HOLD
		motor(RightArm) = 0;
		motor(LeftArm) = 0;
	}
}

task TASKarmstofloor()
{
	while (SensorValue[ArmPot] > ArmsFloor)
	{
		motor(RightArm) = -127;
		motor(LeftArm) = -127;
	}
	motor(RightArm) = 0;
	motor(LeftArm) = 0;
}

task TASKflipperup()
{
	motor(FlipperMotorLeft) = 10;
	motor(FlipperMotorRight) = 10;
	while(true)
	{
		if (SensorValue [FlipperPot] <  2750)
		{
			motor(FlipperMotorLeft) = 60;
			motor(FlipperMotorRight) = 60;
			wait1Msec(10);
			motor(FlipperMotorLeft) = 10;
			motor(FlipperMotorRight) = 10;
		}
		else if (SensorValue [FlipperPot] >  2800)
		{
			motor(FlipperMotorLeft) = -60;
			motor(FlipperMotorRight) = -60;
			wait1Msec(10);
			motor(FlipperMotorLeft) = 10;
			motor(FlipperMotorRight) = 10;
		}
		else
		{
			motor(FlipperMotorLeft) = 10;
			motor(FlipperMotorRight) = 10;
		}
	}
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//	Score the five stack striaght on...
// 	JAWS OPEN!!!!! Back of tile!!! Angled right towards the five stack!!!
//
////////////////////////////////////////////////////////////////////////////////////////
void ScoreFive(bool secondTry)
{
	// flipper down to pick up five stack (opens jaws)
	flippergodown();

	//drive forward to five stack
	MoveFullSpeed(true, 900); //1100

	//StartTask(TASKflipperup);
	StartTask(flipperup);

	// raise arms up to trough
	Arms(true, ArmsTrough, 127);

	MoveFullSpeed(true, 195);//165

	//StopTask(TASKflipperup);
	StopTask(flipperup);

	// score five stack
	flippertotrough();
	motor(LeftArm) = 50;
	motor(RightArm) = 50;
	wait1Msec(500);
	MoveFullSpeed(false, 10);
	motor(RightArm) = 0;
	motor(LeftArm) = 0;

	// TODO - consider calling the arms to floor code here - will need to back up just  a bit first
	StartTask(TASKflippertofloorandhold);
	StartTask(TASKarmstofloor);

	// drive back to starting tile
	MoveFullSpeed(false, 1200);
}

void ScoreHigh()
{
	// arms go from trough to high goal position
	Arms(true, ArmsHigh, 127);

	// flipper moves to straight up and down position
	flipperhigh(true);

	motor[LeftArm]=20;
	motor[RightArm]=20;

	// press button once bags are loaded on top of jaws - coordinate placing the bags on end of flipper with the button push
	WaitForLCD();

	// STRAIGHT SHOT TO HIGH GOAL
	MoveFullSpeed(true, 1310);
	wait1Msec(500);
	// flipper goes down to high goal and rest on high goal - jaws open to push bags to back of high goal
	flipperscorehigh();
	wait1Msec(600);
	// pullback slowly (jaw will be pushing outwards)
	Move(false, 600, SpeedSlow); //500
	//motor(TheJaw) = 0;
	StartTask(TASKflippertofloorandhold);
	StartTask(TASKarmstofloor);
	MoveFullSpeed(false, 1000);
	StopTask(TASKflippertofloorandhold);

	// turn robot towards opposite starting tile
	WaitForLCD();
	StopTask(TASKarmstofloor);
	motor[FlipperMotorLeft]=(127);
	motor[FlipperMotorRight]=(127);
	wait1Msec(1200);
	motor[FlipperMotorLeft]=(0);
	motor[FlipperMotorRight]=(0);

	// move to other tile
	MoveFullSpeed(true, 2000);
}

void MoveToOtherTile()
{
	motor(FlipperMotorLeft) = 127;
	motor(FlipperMotorRight) = 127;
	wait1Msec(700);
	motor(FlipperMotorLeft) = 10;
	motor(FlipperMotorRight) = 10;
	WaitForLCD();
	// move to other tile
	MoveFullSpeed(true, 2000);
}

void ScoreFivePreloads()
{
	// flipper moves tojust above 45 degrees
	flipperhigh(false);

	// line up the bot towards the trough and adjust the flipper and drop in the preloads
	MoveFlipperWithButtons();

	// drive forward
	MoveFullSpeed(true, 1085);

	// score five preloads
	flippertotrough();
	wait1Msec(500);

	// pull away from trough
	MoveFullSpeed(false, 200);

	// start lowering the arms and repositioning the flipper
	StartTask(TASKflippertofloorandhold);
	StartTask(TASKarmstofloor);

	// drive back to starting tile
	MoveFullSpeed(false, 1000);

	//StopTask(TASKflippertofloorandhold);
	//StopTask(TASKarmstofloor);
}

task YellowFlipperHold()
{
	while(1)
	{

		if(SensorValue(FlipperPot) < 2700)//2883
		{
			motor(FlipperMotorLeft) = 50;
			motor(FlipperMotorRight) = 50;
		}
		else if(SensorValue(FlipperPot) > 2800)//3000//3361
		{
			motor(FlipperMotorLeft) = -10;
			motor(FlipperMotorRight) = -10;
		}
		else if(SensorValue(FlipperPot) > 2700 && SensorValue(FlipperPot) < 2800)//3361
		{
			motor(FlipperMotorLeft) = 10;
			motor(FlipperMotorRight) = 10;
		}
		//2883 - 3361 is good
		//if above kick motor up 50
		//	if below kick motor down -10
	}
}



void ScoreYellow(bool secondTry)
{
	// TODO - consider making the arm go down here just in case it gets interrupted in main autonomous loop
	//so far not a problem
	// flipper down to pick up five stack (opens jaws)
	if(secondTry == true)
	{
		while (SensorValue [FlipperPot] > 3040)
		{
			motor(FlipperMotorLeft) = -63;
			motor(FlipperMotorRight) = -63;
		}
		motor(FlipperMotorLeft) = 0;
		motor(FlipperMotorRight) = 0;
	}
	else
	{
		flippergodown();
	}

	if(secondTry == true)
	{
		MoveFullSpeed(true, 1700); //1700
	}
	else
	{
		//drive forward to yellow bag
		MoveFullSpeed(true, /*1500*/1600);
	}
	// raise trough slightly and hold
	motor[FlipperMotorLeft] = 127;
	motor[FlipperMotorRight] = 127;
	MoveFullSpeed(true, 600);
	motor[FlipperMotorLeft] = 10;
	motor[FlipperMotorRight] = 10;

	// back up
	if(secondTry == true)
	{
		MoveLowSpeed(false, 300);
		MoveMediumSpeed(false, 1500);
		//MoveFullSpeed(false, 1700);
	}
	else
	{
		MoveFullSpeed(false, 1400);//1250//900
	}

	if(secondTry == true)
	{
		// dont lower bags on second try cause we are picking up second set of bags that hang on to dear life at the edge and will fall out if we lower them but triforce epicness will sace them
	}
	else
	{
		// lowering flipper so bags dont fall out
		motor[FlipperMotorLeft] = -80;
		motor[FlipperMotorRight] = -80;
		wait1Msec(250);
	}
	motor[FlipperMotorLeft] = 10;
	motor[FlipperMotorRight] = 10;

	// raise arms up to trough
	StartTask(YellowFlipperHold);
	Arms(true, ArmsTrough, 127);

	// drive forward with arms up to trough to score
	if(secondTry == false)
	{
		MoveFullSpeed(true, 350);//230
	}
	else
	{
		MoveFullSpeed(true, 620);//230
	}

	StopTask(YellowFlipperHold);

	flippertotrough();
	motor(LeftArm) = 50;
	motor(RightArm) = 50;
	wait1Msec(500);
	MoveFullSpeed(false, 10);
	motor(RightArm) = 0;
	motor(LeftArm) = 0;

	// drive back to starting tile
	MoveFullSpeed(false, 1200);
}

void PickUpSideBags()
{
	flippergodown();
	//hits wall
	MoveFullSpeed(true, 700);

	wait1Msec(200);
	motor[FlipperMotorLeft] = 127;
	motor[FlipperMotorRight] = 127;
	wait1Msec(700);
	motor[FlipperMotorLeft] = 10;
	motor[FlipperMotorRight] = 10;

	MoveFullSpeed(false, 600);
}

void RunConvayer(int itime, int speed)
{
	motor(Convayer) = speed;
	wait1Msec(itime);
	motor(Convayer) = 0;
}

void ScoreHighGoal()
{
	Arms(true, ArmsHigh, 127);
	motor(LeftArm) = 20;
	motor(RightArm) = 20;
	motor(FlipperMotorLeft) = 20;
	motor(FlipperMotorRight) = 20;
	WaitForLCD();
	MoveFullSpeed(true, 1700);
	RunConvayer(2000, 127);
	StartTask(TASKflippertofloorandhold);
	StartTask(TASKarmstofloor);
	MoveFullSpeed(false, 1800);
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
	// Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
	// Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
	bStopTasksBetweenModes = true;

	//Clear LCD
	clearLCDLine(0);
	clearLCDLine(1);

	//Completely clear out any previous sensor readings by setting the port to "sensorNone"
	//SensorType[in1] = sensorNone;
	//wait1Msec(1000);
	//Reconfigure Analog Port 8 as a Gyro sensor and allow time for ROBOTC to calibrate it
	//SensorType[in1] = sensorGyro;
	//wait1Msec(2000);
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task autonomous()
{
	// Andgle towrads five stack and score it into trough without turning - straight shot
	ScoreFive(false);

	// start lowering the arms and repositioning the flipper
	StartTask(TASKflippertofloorandhold);
	StartTask(TASKarmstofloor);

	WaitForLCD();

	ScoreYellow(false);

	ScoreHighGoal();

	MoveToOtherTile(); // move flipper up (illigal)

	//PickUpSideBags(); // illiAL

	// start lowering the arms and repositioning the flipper
	//StartTask(TASKflippertofloorandhold);
	//StartTask(TASKarmstofloor);

	// angle the robot towards next five stack and press the lcd button
	WaitForLCD();

	while (SensorValue [FlipperPot] < FlipperToFloor)
	{
		motor(FlipperMotorLeft) = -63;
		motor(FlipperMotorRight) = -63;
	}

	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;

	ScoreFive(false);

	// start lowering the arms and repositioning the flipper
	StartTask(TASKflippertofloorandhold);
	StartTask(TASKarmstofloor);

	// angle the robot towards next five stack and press the lcd button
	WaitForLCD();

	// same as step one just on different side of field - straight shot
	ScoreYellow(false);


}


/////////////////////////////////////////////////////////////////////////////////////////
//                                 User Control Task
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
	while (true)
	{
		/*
		motor[RightArm] = -(vexRT(Ch2)/2);
		motor[LeftArm] = -(vexRT(Ch2)/2);
		motor[FlipperMotorRight] = (vexRT(Ch3)/2);
		motor[FlipperMotorLeft] = (vexRT(Ch3)/2);
		*/

		motor(LeftBaseFront) = vexRT(Ch3);
		motor(LeftBaseBack) = vexRT(Ch3);
		motor(RightBaseFront) = vexRT(Ch2);
		motor(RightBaseBack) = vexRT(Ch2);
	}
}
